#include "stdafx.h"
#include "GlobalFunc.h"

//trace pixel value
void GlobalFunc::TraceImageValue(IplImage* pImage)
{
	for (int j=0; j<pImage->height; ++j)
	{
		for (int i=0; i<pImage->width; ++i)
		{
			printf("val: %5f \n", ImageHandler::getPixelValue(pImage, j, i) );
		}
	}
}

void GlobalFunc::PrintByteInBinary(uchar theChar)
{
	uchar uchar_temp=1;
	for (int i=0; i<8; ++i)
	{
		int temp2=theChar & (uchar_temp<<(7-i));
		if (temp2)
		{
			printf("1");
		}
		else
		{
			printf("0");
		}
	}
	printf("\n");
}

void GlobalFunc::TraceMatValue(CvMat* mat)
{
	for (int j=0; j<mat->rows; j++)
	{
		for (int i=0; i<mat->cols; i++)
		{
			uchar temp = CV_MAT_ELEM(*mat,uchar,j,i);
			PrintByteInBinary( temp );
		}
	}
}

MatchResult GlobalFunc::TemplateMatching( ImageHandler& imgTemplate, 
										  ImageHandler& imgTarget, 
										  ImageHandler& depTemplate, 
										  ImageHandler& depTarget)
{
	CvMat* matImgTemplate = imgTemplate.getByteMat();
	CvMat*	matImgTarget = imgTarget.getByteMat();
	CvMat* matDepTemplate = depTemplate.getByteMat();
	CvMat* matDepTarget = depTarget.getByteMat();
	std::vector<Saliency>& cntSaliVec = imgTemplate.getCntKeyPoints();
	std::vector<Saliency>& depSalivec = depTemplate.getDepKeyPoints();
// 	CvMat* matTemplate, matTarget, matDepTemplate, matDepTarget;

	int maxMatchVal=0;
	int maxjPos=0;
	int maxiPos=0;
	int tempVal=0;

	int fullMatchTimes = 0;

	for (int j=0; j<matImgTarget->cols; ++j)
	{
		for (int i=0; i<matImgTarget->rows; ++i)
		{
			tempVal=0;

 			//first salient matching
			int numSaliMatch = 0, numberDepSaliMatch=0;
			for (std::size_t k=0; k<cntSaliVec.size(); ++k)
			{
				Saliency& sali = cntSaliVec[k];
				char charTemplate = CV_MAT_ELEM(*matImgTemplate, uchar, sali.jGPos, sali.iGPos);
				int tempGj = j + sali.jGPos;
				int tempGi = i + sali.iGPos;
				if (tempGj < matImgTarget->rows && tempGi < matImgTarget->cols)
				{
					char charTarget = CV_MAT_ELEM(*matImgTarget, uchar, tempGj, tempGi);
					if ( !(charTemplate & 0X80) )
					{
						numSaliMatch += (charTarget & charTemplate)>0 ? 1:0;
					}
					fullMatchTimes++;
				}
			}
			for (std::size_t k=0; k<depSalivec.size(); ++k)
			{
				Saliency& d_sali = depSalivec[k];
				if (d_sali.hisValue==0)
				{
					continue;
				}
				char charTemplateDep = CV_MAT_ELEM(*matDepTemplate, uchar, d_sali.jGPos, d_sali.iGPos);
				int _tempGj = j + d_sali.jGPos;
				int _tempGi = i + d_sali.iGPos;
				if (_tempGj< matDepTarget->rows && _tempGi < matDepTarget->cols)
				{
					char charTarget = CV_MAT_ELEM(*matDepTarget, uchar, _tempGj, _tempGi);
					if (!(charTemplateDep & 0X80))
					{
						numberDepSaliMatch += (charTarget & charTemplateDep) > 0 ? 1:0;
					}
				}

			}
			if (!(numSaliMatch>=4 && numberDepSaliMatch>=5) )
			{
				continue;
			}
			//lable full match area
			cvRectangle(imgTarget.getImage(), cvPoint(i*GRID_SIZE, j*GRID_SIZE),
						cvPoint(i*GRID_SIZE+imgTemplate.getImage()->width, j*GRID_SIZE+imgTemplate.getImage()->height),
						cvScalar(255, 255, 255));


			for (int _j=j; _j<= j+matImgTemplate->rows-1; ++_j)
			{
				for (int _i=i; _i<=i+matImgTemplate->cols-1; ++_i)
				{
			
					if (_j < matImgTarget->rows && _i < matImgTarget->cols)
					{
						//RGB
						char charTemplate = CV_MAT_ELEM(*matImgTemplate,uchar,_j-j,_i-i);
						if ( !(charTemplate & 0X80) )
						{
							char charTarget = CV_MAT_ELEM(*matImgTarget, uchar, _j, _i);
							tempVal += (charTarget&charTemplate)>0 ? 1:0;
						}
						//depth
						char charDepTemplate = CV_MAT_ELEM(*matDepTemplate, uchar, _j-j, _i-i);
						if (!(charDepTemplate & 0X80))
						{
							char charTarget = CV_MAT_ELEM(*matDepTarget, uchar, _j, _i);
							tempVal += (charTarget&charDepTemplate)>0 ? 1:0;
						}
						fullMatchTimes ++;
					}
				}
			}
			if (tempVal>maxMatchVal)
			{
				maxMatchVal=tempVal;
				maxjPos=j;
				maxiPos=i;
			}
		}
	}
	MatchResult result;
	result.matchVal = maxMatchVal;
	result.jPos = maxjPos;
	result.iPos = maxiPos;

// 	std::cout<<"full matching time: "<<fullMatchTimes<<std::endl;
// 	float match_reduced =  (float(931716-fullMatchTimes))/931716;
// 
// 	std::cout<<"matching time reduced: "<<match_reduced<<std::endl;

	return result;
}

void GlobalFunc::LabelGradient(ImageHandler& imageHandler, int threshold)
{
	int width = imageHandler.getImage()->width;
	int height = imageHandler.getImage()->height;
	int label = 0;
	for (int j=0; j<height; j++)
	{
		for (int i=0; i<width; i++)
		{
			if (ImageHandler::getPixelValue(imageHandler.getSobelMg(), j, i) >= MAGTHRESHOLD )
			{
				ImageHandler::setPixelValue(imageHandler.getImage(), j, i, &label);
			}
		}
	}
}

void GlobalFunc::TestWithTemplateMatching( ImageHandler& imgTemplateHandler, ImageHandler& depTemplateHandler )
{
	hlTimeCounter tTimeCounter;
	cvNamedWindow("winCap",CV_WINDOW_AUTOSIZE);

	//build font to show frame rate:
	CvFont	font;
	cvInitFont(&font, CV_FONT_HERSHEY_COMPLEX, 0.7, 0.7, 0, 2.0);
	float	frameRate=0.0;
	char	buffer[100];

	int		nFrame=0;
	int		key=0;
	bool	startFF=false;
	bool	startLabel=false;
	int		flag=0;
	ImageHandler capImagehandler;
	ImageHandler capDepthhandler;

	//test use camera
//	CvCapture* pCapture=cvCaptureFromCAM(-1);
//	IplImage *pCapImage=NULL;
// 	cvQueryFrame(pCapture);  
// 	cvSetCaptureProperty(pCapture, CV_CAP_PROP_FRAME_HEIGHT, 480);
// 	cvSetCaptureProperty(pCapture, CV_CAP_PROP_FRAME_WIDTH, 640);
// 	while (pCapImage= cvQueryFrame( pCapture ))

	//use Kinect
	VideoCapture capture(CV_CAP_OPENNI);
	if( !capture.isOpened())
	{
		cout << "Can not open a capture object." << endl;
		return ;
	}
	Mat greyMap;
	Mat depthMap;

	while (1)
	{
		capture.grab();
		capture.retrieve(greyMap, CV_CAP_OPENNI_GRAY_IMAGE);
		capture.retrieve(depthMap, CV_CAP_OPENNI_DEPTH_MAP);

		IplImage _pGreyImage = greyMap;
		IplImage* pGreyImage = &_pGreyImage; 
		IplImage _pDepImage = depthMap;
		IplImage* pDepImage = GlobalFunc::convertTo32FImg(&_pDepImage);

		//set timer
		tTimeCounter.Reset();
		tTimeCounter.Start();

		//convert to grey image
// 		IplImage *pGreyImage=cvCreateImage(cvGetSize(pCapImage), IPL_DEPTH_8U, 1);
// 		cvCvtColor(pCapImage, pGreyImage, CV_BGR2GRAY);

		//construct and build target image
		capImagehandler.setImageWithPointer(pGreyImage);
		capImagehandler.constructGradientImage();
		capImagehandler.initGridInfo();
		capImagehandler.generateImgTargetByteMat();


		capDepthhandler.setImageWithPointer(pDepImage);
		capDepthhandler.constructGradientImage();
		capDepthhandler.initGridInfo();
		capDepthhandler.generateDepTargetByteMat();


		//show labeled image
		if (startLabel)
		{
			LabelGradient(capImagehandler, MAGTHRESHOLD);
		}


		//match template
		MatchResult result = TemplateMatching(imgTemplateHandler,
												capImagehandler,
												depTemplateHandler,
												capDepthhandler);
		tTimeCounter.End();
		//do frame to frame tracking: FF
		if (startFF /*& !flag*/)
		{
			flag=1;
			CvSize imgSize = cvGetSize(imgTemplateHandler.getImage());
			imgTemplateHandler.releaseAll();
			imgTemplateHandler.createImage(imgSize, IPL_DEPTH_8U, 1);

			int pix_y=result.jPos*GRID_SIZE;
			int pix_x=result.iPos*GRID_SIZE;

			for (int j=0; j<imgTemplateHandler.getImage()->height; j++)
			{
				for (int i=0; i<imgTemplateHandler.getImage()->width; i++)
				{
					char val = ImageHandler::getPixelValue(capImagehandler.getImage(), pix_y+j, pix_x+i);
					ImageHandler::setPixelValue(imgTemplateHandler.getImage(), j, i, &val );
				}
			}
			imgTemplateHandler.constructGradientImage();
			imgTemplateHandler.initGridInfo();
			imgTemplateHandler.generateImgTemplateByteMat();
		}

		//draw rectangle
		cvRectangle(capImagehandler.getImage(), 
					cvPoint(result.iPos*GRID_SIZE,result.jPos*GRID_SIZE),
					cvPoint(result.iPos*GRID_SIZE+imgTemplateHandler.getImage()->width,
					result.jPos*GRID_SIZE+imgTemplateHandler.getImage()->height), 
					cvScalar(255,255,255));


		//calculate and print frame rate
		frameRate=1.0/tTimeCounter.getElapseTime();
		sprintf(buffer, "%.2f", frameRate);
		cvPutText(capImagehandler.getImage(), buffer, cvPoint(550, 400), &font, cvScalar(0,0,0));

		//show the image;
		cvShowImage("winCap", capImagehandler.getImage());

		key=cvWaitKey(1);
		switch(key){
		case 'f':
			startFF=!startFF;
			cout<<"adjust start"<<endl;
			break;
		case 'l': 
			startLabel=!startLabel;
			cout<<"label gradient start"<<endl;
		default:
			break;
		}
		//release all
		greyMap.release();
		capImagehandler.setImageWithPointer(NULL);
		capImagehandler.releaseAll();
		depthMap.release();
		capDepthhandler.releaseAll();
	}
	cvDestroyAllWindows();
// 	cvReleaseCapture(&pCapture);
}

void GlobalFunc::buildMouseCallBack(int mouseEvent, int x, int y, int flags, void* param)
{
	DrawRect* drawRect = (DrawRect*)param;
	switch (mouseEvent)
	{
	case CV_EVENT_LBUTTONDOWN:
		std::cout<<"down:x: "<<x<<std::endl;
		std::cout<<"down:y: "<<y<<std::endl;
		drawRect->point1 = cvPoint(x, y);
		drawRect->point2 = cvPoint(x, y);
		drawRect->isDraw =true;
		break;
	case  CV_EVENT_MOUSEMOVE:
		drawRect->point2 = cvPoint(x, y);
		break;
	case  CV_EVENT_LBUTTONUP:
		std::cout<<"up:x: "<<x<<std::endl;
		std::cout<<"up:y: "<<y<<std::endl;
		grapTemplateImage(drawRect->point1, drawRect->point2, drawRect->greyImage, drawRect->depthImage);
		break;
	default:
		break;
	}
}

void GlobalFunc::grapTemplateImage(CvPoint startPoint, CvPoint endPoint, IplImage* greyImage, IplImage* depthImage)
{
	int start_h = startPoint.y;
	int start_w = startPoint.x;
	int end_h = endPoint.y;
	int end_w = endPoint.x;
	int imgWidth = end_w - start_w + 1;
	int imgHeight = end_h - start_h + 1;
	IplImage* greyTemplate = cvCreateImage(cvSize(imgWidth, imgHeight), greyImage->depth, greyImage->nChannels);
	IplImage* depthTempalte = cvCreateImage(cvSize(imgWidth, imgHeight), depthImage->depth, depthImage->nChannels);
	for (int j = start_h; j< start_h + imgHeight; ++j)
	{
		for (int i = start_w; i < start_w + imgWidth; ++i)
		{
			uchar greyVal = ImageHandler::getPixelValue(greyImage, j, i);
			ImageHandler::setPixelValue(greyTemplate, j-start_h, i-start_w, &greyVal);
			ushort depthVal = ImageHandler::getPixelValue(depthImage, j, i);
			ImageHandler::setPixelValue(depthTempalte, j - start_h, i - start_w, &depthVal);
		}
	}
	cvSaveImage("image\\template\\imgTemp.png", greyTemplate);
	cvSaveImage("image\\template\\depTemp.png", depthTempalte);
}

void GlobalFunc::CapWithKinect()
{
	DrawRect dRect;
	dRect.imgTemp = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U, 1);
	dRect.greyImage = NULL;
	dRect.depthImage = NULL;
	dRect.point1 = cvPoint(0, 0);
	dRect.point2 = cvPoint(0, 0);
	dRect.isDraw = false;

	cvNamedWindow("kColorCap", CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("kDepthCap", CV_WINDOW_AUTOSIZE);
	cvSetMouseCallback("kColorCap", buildMouseCallBack, &dRect);

	//use Kinect
	VideoCapture capture(CV_CAP_OPENNI);
	if( !capture.isOpened())
	{
		cout << "Can not open a capture object." << endl;
		return ;
	}

	Mat depthMap;
	Mat greyMap;
	
	while (1)
	{
		capture.grab();
		capture.retrieve(greyMap, CV_CAP_OPENNI_GRAY_IMAGE );
		capture.retrieve(depthMap, CV_CAP_OPENNI_DEPTH_MAP );
// 		capture.retrieve(rgbMap, CV_CAP_OPENNI_GRAY_IMAGE );
// 		cv::imshow( "kColorCap", greyMap );
// 		cv::imshow( "kDepthCap", depthMap);
		IplImage temp = greyMap;
		dRect.imgTemp = cvCloneImage(&temp);
		dRect.greyImage = &temp;
		IplImage capDepth = depthMap;
		dRect.depthImage = &capDepth;
		if (dRect.isDraw)
		{
			cvRectangle(dRect.imgTemp, dRect.point1, dRect.point2, CV_RGB(255, 255, 255));
		}
		cvShowImage("kColorCap", dRect.imgTemp);

// 		grapTemplateImage(cvPoint(305,262), cvPoint(362,355), dRect.greyImage, dRect.depthImage);

		if (cvWaitKey(20) == 'q')
		{
			depthMap.release();
			greyMap.release();
			cvReleaseImage(&(dRect.imgTemp));
			cvDestroyAllWindows();
			return;
		}
		depthMap.release();
		greyMap.release();
		cvReleaseImage(&(dRect.imgTemp));
	}
}

IplImage* GlobalFunc::convertTo32FImg( IplImage* src  )
{
	IplImage* resultImg = cvCreateImage(cvGetSize(src), IPL_DEPTH_32F, 1);
	if(src->depth >= 8)
	{
		int width = src->width;
		int height = src->height;
		for (int j=0; j<height; ++j)
		{
			for (int i=0; i<width; ++i)
			{
				ushort data = ImageHandler::getPixelValue(src, j,i);
				ImageHandler::setPixelValue(resultImg, 
											j,
											i,
											&data);
			}
		}
	}
	else
	{
		int width = src->width;
		int height = src->height;
		for (int j=0; j<height; ++j)
		{
			for (int i=0; i<width; ++i)
			{
				ushort data;
				CvScalar s = cvGet2D(src,j,i);
				if(s.val[0])
				{
					data = 1; 
				}
				else
					data = 0;
				ImageHandler::setPixelValue(resultImg, 
					j,
					i,
					&data);
			}
		}

	}
	return resultImg;
}


void GlobalFunc::CapWithCamera()
{

}

void GlobalFunc::TraceGridValue(IplImage* pImg, GRID_POS jGPos, GRID_POS iGPos )
{
	int j_start = jGPos * GRID_SIZE;
	int i_start = iGPos * GRID_SIZE;
	for ( int j=j_start; j< j_start + GRID_SIZE; ++j)
	{
		for (int i=i_start; i< i_start + GRID_SIZE; ++i)
		{
			ushort val = ImageHandler::getPixelValue(pImg, j, i);
			int v =   val;
			printf("%d ", v);
		}
		printf("\n");
	}
}

IplImage* GlobalFunc::scaleToGreyScale( IplImage* src)
{
	int width = src->width;
	int height = src->height;
	IplImage* greyImage = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
	float max=0.0, min=99999999999.0;
	for (int j=0; j<height; ++j)
	{
		for (int i=0; i<width; ++i)
		{
			ushort val = ImageHandler::getPixelValue(src, j, i);
			if (val>max)
			{
				max = val;
			}
			if (val<min)
			{
				min = val;
			}
		}
	}
	float interval = max - min;
	for (int j=0; j<height; ++j)
	{
		for (int i=0; i<width; ++i)
		{
			ushort val = ImageHandler::getPixelValue(src, j, i);
			int greyVal = (val/interval) * 255;
			ImageHandler::setPixelValue(greyImage, j, i, &greyVal);
		}
	}
	return greyImage;
}

IplImage* GlobalFunc::buildMaskImage( IplImage* rgbTmpalte, IplImage* rgbBack, IplImage* depTemplate, IplImage* depBack )
{
	int width = rgbTmpalte->width;
	int height = rgbTmpalte->height;
	IplImage* maskImage = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
	for (int j =0; j<height; ++j)
	{
		for (int i=0; i<width; ++i)
		{
			uchar rgbForVal = ImageHandler::getPixelValue(rgbTmpalte, j, i);
			uchar rgbBackVal = ImageHandler::getPixelValue(rgbBack, j, i);
			ushort depForVal = ImageHandler::getPixelValue(depTemplate, j, i);
			ushort depBackVal = ImageHandler::getPixelValue(depBack, j, i);
			int val;
			if(depForVal ==0)
			{
				std::cout<<"dep for val"<<std::endl;
				if (std::abs(int(rgbBackVal-rgbForVal)) > 10)
				{
					val = 255;
				}
				else
					val = 0;
			}
			else
			{
				if (std::abs(int(depBackVal - depForVal) > 10))
				{
					val = 255;
				}
				else
					val =0;
			}
			ImageHandler::setPixelValue(maskImage, j, i, &val);

		}
	}
	return maskImage;
}

